﻿using System;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using VIA.Integration;
using VIA.Integration.Alarms;

namespace VIA.Integration.LeadShine
{
    public class DmcAxis : ServoAxis, IHasOption<AxisOption>, ISuportInitialization, ISuportClearAlarm, IRefreshing
    {
        private MotionPath<string, VelocityCurve> _lastMotionPath;
        protected DMCController Controller { get; }
        private volatile ushort usDir = 0;
        private bool _isMoveTimeoutError;
        private bool _isStatusError;
        private bool _isHomeError;
        private bool _isMoveError;
        private bool _isELPError;
        private bool _isELNError;
        /// <summary>
        /// 当前速度
        /// </summary>
        public double CurrentSpeed => Controller.GetSpeed((ushort)NoId);
        public override double Current => Controller.GetCurrentPosition((ushort)NoId);
        public override bool IsDone => Controller.CheckIsDone((ushort)NoId);
        public AxisRunningMode RunningMode => Controller.GetRunningMode((ushort)NoId);
        /// <summary>
        /// 是否为上使能状态
        /// </summary>
        public bool IsServoOn => Controller.GetAxisState((ushort)NoId) == DmcAxisState.OP_ENABLE;

        [Alarm(true, AlarmLevel.Error, Description = "轴移动超时")]
        public bool IsMoveTimeoutError { get => _isMoveTimeoutError; set => this.NotifyAlarm(ref _isMoveTimeoutError, value); }
        [Alarm(true, AlarmLevel.Error, Description = "轴状态报警")]
        public bool IsStatusError { get => _isStatusError; set => this.NotifyAlarm(ref _isStatusError, value); }
        [Alarm(true, AlarmLevel.Error, Description = "轴回原点失败")]
        public bool IsHomeError { get => _isHomeError; set => this.NotifyAlarm(ref _isHomeError, value); }
        [Alarm(true, AlarmLevel.Error, Description = "轴安全条件限制不能移动")]
        public bool IsMoveError { get => _isMoveError; set => this.NotifyAlarm(ref _isMoveError, value); }

        [Alarm(true, AlarmLevel.Error, Description = "轴正极限报警")]
        public bool IsELPError { get => _isELPError; set => this.NotifyAlarm(ref _isELPError, value); }
        [Alarm(true, AlarmLevel.Error, Description = "轴负极限报警")]
        public bool IsELNError { get => _isELNError; set => this.NotifyAlarm(ref _isELNError, value); }
        AxisStatus _status;
        public override AxisStatus Status
        {
            get => _status;
            protected set => this.NotifyAlarm(ref _status, value);
        }

        public Func<bool> CanMove;

        public AxisOption Option { get; set; }

        public DmcAxis(DMCController controller)
        {
            Controller = controller;
            this.InitalizeAlarms();
        }

        public override async Task MoveToAsync(double value, VelocityCurve velocityCurve = null, CancellationToken cancelToken = default)
        {
            //if (CanMove != null)
            //    if (!CanMove.Invoke())
            //        throw new LeadShineException($"{Name}安全条件限制不能移动", 999);
            if (!await IsCheckCanMoveAsync()) return;
            if (!IsCheckSafetyLimit()) return;
            Controller.AbsoluteMove((ushort)NoId, value, velocityCurve);
            await Judger.Sure(() => IsDone, cancelToken);
        }

        public override async Task MoveDeltaAsync(double value, VelocityCurve velocityCurve = null, CancellationToken cancelToken = default)
        {
            //if (CanMove != null)
            //    if (!CanMove.Invoke())
            //        throw new LeadShineException($"{Name}安全条件限制不能移动", 999);
            if (!await IsCheckCanMoveAsync()) return;
            if (!IsCheckSafetyLimit()) return;
            if (cancelToken != null)
                cancelToken.ThrowIfCancellationRequested();
            Controller.RelativeMove((ushort)NoId, value, velocityCurve);
            await Judger.Sure(() => IsDone, cancelToken);
        }

        VelocityCurve GetDefaultVelocityCurve() => Option?.VelocityCurve;
        public override async Task PostiveAsync(VelocityCurve velocityCurve = null, CancellationToken cancelToken = default)
        {
            //if (CanMove != null)
            //    if (!CanMove.Invoke())
            //        throw new LeadShineException($"{Name}安全条件限制不能移动", 999);
            if (!await IsCheckCanMoveAsync()) return;
            if (!IsCheckSafetyLimit()) return;
            //if (cancelToken.IsCancellationRequested) return;
            if (cancelToken != null)
                cancelToken.ThrowIfCancellationRequested();
            velocityCurve ??= GetDefaultVelocityCurve();
            Controller.ContinuousMove((ushort)NoId, MoveDirection.Postive, velocityCurve);
        }

        public override async Task NegativeAsync(VelocityCurve velocityCurve = null, CancellationToken cancelToken = default)
        {
            //if (CanMove != null)
            //    if (!CanMove.Invoke())
            //        throw new LeadShineException($"{Name}安全条件限制不能移动", 999);
            if (!await IsCheckCanMoveAsync()) return;
            if (!IsCheckSafetyLimit()) return;
            //if (cancelToken.IsCancellationRequested) return;
            if (cancelToken != null)
                cancelToken.ThrowIfCancellationRequested();
            velocityCurve ??= GetDefaultVelocityCurve();
            Controller.ContinuousMove((ushort)NoId, MoveDirection.Negative, velocityCurve);
        }

        public override async Task PauseAsync(VelocityCurve velocityCurve = null, CancellationToken cancelToken = default)
        {
            _lastMotionPath = new MotionPath<string, VelocityCurve>
            {
                Target = Controller.GetCommandPosition((ushort)NoId),
                VelocityCurve = Controller.GetAxisVelocity((ushort)NoId)
            };
            Controller.DecelStop((ushort)NoId);
            await Judger.Sure(() => IsDone, cancelToken);
        }
        /// <summary>
        ///     轴停止运动。
        /// </summary>
        /// <param name="velocityCurve"></param>
        /// <param name="cancelToken"></param>
        public async Task EmgStopAsync(VelocityCurve velocityCurve = null,
            CancellationToken cancelToken = default(CancellationToken))
        {
            Controller.EmgStop((ushort)NoId);
            await Judger.Sure(() => IsDone, cancelToken);
        }
        public override async Task ResumeAsync(CancellationToken cancelToken = default)
        {
            await MoveToAsync(_lastMotionPath, cancelToken);
        }

        /// <summary>
        /// 检测轴是否在指定位置
        /// </summary>
        /// <param name="position">指定位置</param>
        /// <param name="tolerance">误差允许</param>
        /// <returns></returns>
        public bool CheckIsInPosition(double position, double tolerance = 0.5)
        {
            var ps = Current;
            return Math.Abs(ps - position) < tolerance;
        }

        /// <summary>
        /// 检测轴是否在指定名称所在位置
        /// </summary>
        /// <param name="position">轴位置名称</param>
        /// <param name="tolerance">误差允许</param>
        /// <returns></returns>
        public bool CheckIsInPosition(string position, double tolerance = 0.5)
        {
            var path = this.MotionPaths.FirstOrDefault(x => x.Id == position);
            if (path == null)
                throw new Exception($"没有找到名为{position}的点位路径");
            var ps = Current;
            return Math.Abs(ps - path.Target) < tolerance;
        }
        /// <summary>
        /// 获取当前软限位值，根据脉冲当量换算为mm
        /// </summary>
        /// <param name="nLimit">返回负限位mm</param>
        /// <param name="pLimit">返回正限位mm</param>
        /// <param name="enable">返回使能状态0：禁止，1：允许</param>
        /// <param name="source_sel">返回计数器选择，0：指令位置计数器，1：编码器计数器</param>
        /// <param name="SL_action">返回限位停止方式，0：立即停止 1：减速停止</param>
        public SoftLimitValue GetSoftLimit() => Controller.GetSoftLimit((ushort)NoId);



        /// <summary>
        /// 是否有ALarm
        /// </summary>
        /// <returns></returns>
        public bool IsAlarm()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = data & 1;
            return v > 0;
        }
        /// <summary>
        /// 正硬限位
        /// </summary>
        /// <returns></returns>
        public bool IsELP()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 1) & data);
            return v > 0;
        }
        /// <summary>
        /// 负硬限位
        /// </summary>
        /// <returns></returns>
        public bool IsELN()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 2) & data);
            return v > 0;
        }
        /// <summary>
        /// 急停信号
        /// </summary>
        /// <returns></returns>
        public bool IsEMG()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 3) & data);
            return v > 0;
        }
        /// <summary>
        /// 原点信号
        /// </summary>
        /// <returns></returns>
        public bool IsORG()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 4) & data);
            return v > 0;
        }
        /// <summary>
        /// 正软限位
        /// </summary>
        /// <returns></returns>
        public bool IsSLP()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 6) & data);
            return v > 0;
        }
        /// <summary>
        /// 负软限位信号
        /// </summary>
        /// <returns></returns>
        public bool IsSLN()
        {
            uint data = Controller.GetIOStatus((ushort)NoId);
            var v = ((1 << 7) & data);
            return v > 0;
        }
        /// <summary>
        /// 清除报警
        /// </summary>
        public void ClearAlarm()
        {
            IsMoveTimeoutError = false;
            IsStatusError = false;
            IsHomeError = false;
            IsMoveError = false;
            IsELPError = false;
            IsELNError = false;
            Controller.ClearAlarm((ushort)NoId);
        }
        public (VelocityCurve VelocityCurve, double HomeOffset) GetHomeVelocityCurve()
        {
            return Controller.GetHomeVelocityCurve((ushort)NoId);
        }
        public VelocityCurve GetCurrentVelocityCurve()
        {
            return Controller.GetAxisVelocity((ushort)NoId);
        }
        public override async Task MoveOriginAsync(CancellationToken cancelToken = default)
        {
            if (Option != null && Option.HomeVelocityCurve != null)
                await MoveOriginAsync(Option.HomeVelocityCurve, Option.HomeOffset, cancelToken);
            else
                await MoveOriginAsync(null, 0, cancelToken);
        }
        public async Task MoveOriginAsync(VelocityCurve velocityCurve, double homeOffset, CancellationToken cancelToken = default)
        {
            if (cancelToken != null)
                cancelToken.ThrowIfCancellationRequested();
            Controller.MoveOrigin((ushort)NoId, velocityCurve, Option.HomeOffset);
            await Judger.Sure(() =>
            {
                bool b = Controller.CheckHomeDone((ushort)NoId);
                return b;
            }, cancelToken);
            Controller.SetOriginal((ushort)NoId, 0);
        }
        public async Task<bool> MoveOriginAsync2(CancellationToken cancelToken = default, int timeout = Timeout.Infinite)
        {
            if (Option != null && Option.HomeVelocityCurve != null)
                return await MoveOriginAsync2(Option.HomeVelocityCurve, Option.HomeOffset, cancelToken, timeout);
            else
                return await MoveOriginAsync2(null, 0, cancelToken, timeout);
        }
        public async Task<bool> MoveOriginAsync2(VelocityCurve velocityCurve, double homeOffset, CancellationToken cancelToken = default, int timeout = Timeout.Infinite)
        {
            if (cancelToken != null)
                cancelToken.ThrowIfCancellationRequested();
            Controller.MoveOrigin((ushort)NoId, velocityCurve, Option.HomeOffset);
            var state = await Judger.Sure(() =>
            {
                bool b = Controller.CheckHomeDone((ushort)NoId);
                return b;
            }, cancelToken, timeout: timeout);
            if (state)
            {
                Controller.SetOriginal((ushort)NoId, 0);
            }
            return state;
        }

        public override void Reset()
        {
            Controller.ClearAlarm((ushort)NoId);
        }

        public override void ServoOn(bool isOn)
        {
            if (isOn)
                Controller.ServoOn((ushort)NoId);
            else
                Controller.ServoOff((ushort)NoId);
        }
        /// <summary>
        /// 在线改变指定轴的当前运动速度
        /// </summary>
        /// <param name="speed">新的速度</param>
        /// <param name="taccdec">加速度/减速度</param>
        /// <returns></returns>
        public void ChangeSpeed(double speed, double taccdec = 0)
        {
            Controller.ChangeSpeed((ushort)NoId, speed, taccdec);
        }

        public Task InitializeAsync()
        {
            Option = Recipe?.Get<AxisOption>(this.Id);
            this.Speed = Option.VelocityCurve?.Maxvel ?? 1;
            this.Equivalent = Option.Equivalent;
            this.NoId = Option.NoId;

            this.MotionPaths.Clear();
            foreach (var mpo in Option.MotionPaths)
            {
                this.MotionPaths.Add(new MotionPath<string, VelocityCurve>
                {
                    Id = mpo.Id,
                    Name = mpo.Id,
                    Target = mpo.Target,
                    VelocityCurve = mpo.VelocityCurve
                });
            }
            return Task.CompletedTask;
        }
        public void RefreshIoStatus()
        {
            var ioStatus = Controller.GetIOStatus((ushort)NoId);
            AxisStatus status = AxisStatus.None;

            //if (((ioStatus >> (int)DMC_Define.MIO_SVON) & 1) > 0)
            //{
            //    status = status | AxisStatus.ServoOn;
            //}
            if (((ioStatus >> (int)DMC_Define.ALM) & 1) > 0)
            {
                status = status | AxisStatus.Alarmed;
            }
            if (((ioStatus >> (int)DMC_Define.EMG) & 1) > 0)
            {
                status = status | AxisStatus.EmgStop;
            }
            if (((ioStatus >> (int)DMC_Define.ELN) & 1) > 0)
            {
                status = status | AxisStatus.NegativeLimit;
            }
            if (((ioStatus >> (int)DMC_Define.ORG) & 1) > 0)
            {
                status = status | AxisStatus.Origin;
            }
            if (((ioStatus >> (int)DMC_Define.ELP) & 1) > 0)
            {
                status = status | AxisStatus.PositiveLimit;
            }
            this.Status = status;
        }

        public void Refresh()
        {
            RefreshIoStatus();
        }

        public void Refreshing()
        {
        }
        /// <summary>
        /// 检测轴是否在安全条件下移动
        /// </summary>
        /// <returns></returns>
        protected async Task<bool> IsCheckCanMoveAsync()
        {
            if (CanMove != null)
            {
                if (!CanMove.Invoke())
                {
                    if (!_isMoveError)
                        IsMoveError = true;
                    await Task.Delay(1000);
                    return false;
                }
            }
            return true;
        }
        /// <summary>
        /// 检测轴是否在正负硬极限范围内
        /// </summary>
        /// <returns></returns>
        protected bool IsCheckSafetyLimit()
        {
            if (IsELP() || IsSLP())
            {
                IsELPError = true;
            }
            if (IsELN() || IsSLN())
            {
                IsELNError = true;
            }
            if (_isELPError || _isELNError)
            {
                return false;
            }
            return true;
        }
    }
}
